#!/usr/bin/env python3

import rospy
import serial
import time
import pynmea2
from sensor_msgs.msg import NavSatFix, NavSatStatus
from std_msgs.msg import String


class RTKReaderNode:
    def __init__(self):
        rospy.init_node('rtk_reader_node')

        # ROS参数
        self.port = rospy.get_param('~port', '/dev/ttyUSB0')
        self.baudrate = rospy.get_param('~baudrate', 115200)
        self.frame_id = rospy.get_param('~frame_id', 'rtk_link')
        self.publish_raw = rospy.get_param('~publish_raw', False)

        # ROS发布者
        self.navsat_pub = rospy.Publisher('~navsat', NavSatFix, queue_size=10)
        if self.publish_raw:
            self.raw_pub = rospy.Publisher('~raw', String, queue_size=10)

        # 串口初始化
        try:
            self.ser = serial.Serial(self.port, self.baudrate, timeout=1)
            rospy.loginfo(f"串口 {self.port} 已打开")
        except serial.SerialException as e:
            rospy.logerr(f"串口打开失败：{e}")
            rospy.signal_shutdown("串口打开失败")
            return

        self.buffer = ""
        self.run()

    def run(self):
        rate = rospy.Rate(10)  # 10 Hz
        while not rospy.is_shutdown():
            try:
                if self.ser.in_waiting > 0:
                    data = self.ser.read(self.ser.in_waiting).decode('utf-8', errors='ignore')
                    self.buffer += data

                    while '\n' in self.buffer:
                        line, self.buffer = self.buffer.split('\n', 1)
                        line = line.strip()
                        if line:
                            self.process_nmea_data(line)

            except serial.SerialException as e:
                rospy.logerr(f"串口读取错误：{e}")
                if self.ser.is_open:
                    self.ser.close()
                break
            except Exception as e:
                rospy.logerr(f"发生未知错误: {e}")
                break

            rate.sleep()

        if self.ser.is_open:
            self.ser.close()
            rospy.loginfo("串口已关闭")

    def process_nmea_data(self, line):
        """处理NMEA数据"""
        try:
            msg = pynmea2.parse(line)

            if self.publish_raw:
                self.raw_pub.publish(line)

            # 处理GGA消息
            if isinstance(msg, pynmea2.GGA):
                self.process_gga(msg)

        except pynmea2.ParseError as e:
            rospy.logwarn(f"NMEA解析错误: {e}")
        except Exception as e:
            rospy.logerr(f"处理数据时发生未知错误: {e}")

    def process_gga(self, msg):
        """处理GGA消息并发布NavSatFix消息"""
        navsat_msg = NavSatFix()
        navsat_msg.header.stamp = rospy.Time.now()
        navsat_msg.header.frame_id = self.frame_id

        # 检查Fix Quality
        if msg.gps_qual == 0:
            navsat_msg.status.status = NavSatStatus.STATUS_NO_FIX
        elif msg.gps_qual == 1:
            navsat_msg.status.status = NavSatStatus.STATUS_FIX
        elif msg.gps_qual == 2:
            navsat_msg.status.status = NavSatStatus.STATUS_SBAS_FIX
        elif msg.gps_qual in (4, 5):
            navsat_msg.status.status = NavSatStatus.STATUS_GBAS_FIX  # RTK fixed or float
        else:
            navsat_msg.status.status = NavSatStatus.STATUS_FIX

        navsat_msg.status.service = NavSatStatus.SERVICE_GPS  # 假设是GPS
        navsat_msg.latitude = msg.latitude
        navsat_msg.longitude = msg.longitude

        # 确保高度是浮点数
        try:
            altitude = float(msg.altitude)  # 尝试转换为浮点数
        except (TypeError, ValueError) as e:
            rospy.logwarn(f"无法将高度转换为浮点数: {msg.altitude}, 错误信息: {e}。设置高度为0.0")
            altitude = 0.0  # 如果转换失败，设置为默认值0.0
        navsat_msg.altitude = altitude

        # 添加协方差 (可选)
        # navsat_msg.position_covariance = [...]
        # navsat_msg.position_covariance_type = ...

        self.navsat_pub.publish(navsat_msg)

if __name__ == '__main__':
    try:
        rtk_reader = RTKReaderNode()
    except rospy.ROSInterruptException:
        pass